#ifndef WALL_LOCALIZATION__WALL_LOCALIZER_HPP_
#define WALL_LOCALIZATION__WALL_LOCALIZER_HPP_


#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp> 
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h> 
#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include <random>
#include <algorithm>
#include "wall_localization/pose_json.hpp"

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <mutex>

namespace Wall_Localization
{
    struct Line
    {
        //直线表达式：ax + by + c = 0
        double a, b, c;
        bool valid;
    };

    struct WallInfo
    {
        Line line;
        double distance;
        Eigen::Vector2d normal;
        Eigen::Vector2d center;
        int point_count;
        bool valid;
        std::vector<Eigen::Vector2d> points;
    };
    
    struct Pose2D
    {
        double x;
        double y;
        double theta;
    };

    class WallLocalizer : public rclcpp::Node
    {
    public:
        WallLocalizer();

    private:

        void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);

        // 位姿话题和json发布
        void poseTimerCallback();

        // 单线雷达数据转换
        // 将单线激光雷达坐标系的点云转换到机器人本体坐标系下
        std::vector<Eigen::Vector2d> convertLaserScanToBaseLinkPoints(const sensor_msgs::msg::LaserScan::SharedPtr msg);

        // 基于机器人坐标系分区
        // 计算墙壁信息
        std::vector<std::vector<Eigen::Vector2d>> segmentWalls(const std::vector<Eigen::Vector2d>& points);
        Line fitLineRANSAC(const std::vector<Eigen::Vector2d>& points, double threshold = 0.05, int max_iterations = 100);
        Line fitLineTwoPoints(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2);
        double pointToLineDistance(const Eigen::Vector2d& point, const Line& line);
        int countInliers(const std::vector<Eigen::Vector2d>& points, const Line& line, double threshold);
        Eigen::Vector2d calculateNormal(const Line& line);
        Eigen::Vector2d calculateLineCenter(const std::vector<Eigen::Vector2d>& points);
        void printWallInfo(const std::vector<WallInfo>& walls);

        // 墙体检测和位姿估计
        std::vector<WallInfo> detectWalls(const std::vector<Eigen::Vector2d>& points);
        void estimatePoseFromWalls(const std::vector<WallInfo>& walls);
        Eigen::Vector2d ensureInwardNormal(const Eigen::Vector2d& normal,
                                        const Eigen::Vector2d& centroid, 
                                        const std::vector<Eigen::Vector2d>& points);
        double calculateRobotOrientationFromWalls(const WallInfo& left_wall, const WallInfo& back_wall);


        // ros2相关
        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
        rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_json_pub_;

        // 参数
        double warehouse_width_;
        double warehouse_height_;
        double laser_offset_x_;
        double laser_offset_y_;
        double ransac_distance_threshold_;
        int ransac_max_iterations_;


        // 状态
        Pose2D current_pose_;
        bool pose_valid_ = false;
        sensor_msgs::msg::LaserScan::SharedPtr latest_scan_;

        // 定时输出
        rclcpp::TimerBase::SharedPtr pose_timer_;
        geometry_msgs::msg::PoseWithCovarianceStamped latest_pose_;
        std::mutex pose_mutex_;

    };

}

#endif